#include <Servo.h>  // ESP32 需要使用 Servo 库

// 机械臂连杆长度（单位：cm）
const float L1 = 10.0;  // 第一个连杆长度
const float L2 = 10.0;  // 第二个连杆长度

// 舵机引脚定义
#define BASE_SERVO_PIN  13  // 底座旋转舵机（θ1）
#define SHOULDER_SERVO_PIN  12  // 肩部舵机（θ2）
#define ELBOW_SERVO_PIN  14  // 肘部舵机（θ3）
#define WRIST_SERVO_PIN  27  // 末端舵机（θ4）

// 舵机对象
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo wristServo;

// 目标位置（单位：cm）
float target_x = 10.0;
float target_y = 5.0;
float target_z = 10.0;

// 计算逆运动学
void computeIK(float x, float y, float z, float &theta1, float &theta2, float &theta3, float &theta4) {
    // 计算底盘旋转角度 θ1
    theta1 = atan2(y, x) * 180.0 / PI;  // 转换为度数

    // 计算目标点到机械臂的水平距离
    float r = sqrt(x*x + y*y);  

    // 计算 θ3（肘部角度） - 余弦定理
    float D = (r*r + z*z - L1*L1 - L2*L2) / (2 * L1 * L2);
    theta3 = acos(D) * 180.0 / PI;

    // 计算 θ2（肩部角度）
    float alpha = atan2(z, r) * 180.0 / PI;
    float beta = acos((L1*L1 + r*r + z*z - L2*L2) / (2 * L1 * sqrt(r*r + z*z))) * 180.0 / PI;
    theta2 = alpha - beta;

    // 计算末端角度 θ4，使其保持水平
    theta4 = -(theta2 + theta3);
}

// 将角度映射到舵机范围（0° - 180°）
int mapServoAngle(float angle) {
    return constrain(int(angle), 0, 180);
}

void setup() {
    Serial.begin(115200);

    // 初始化舵机
    baseServo.attach(BASE_SERVO_PIN);
    shoulderServo.attach(SHOULDER_SERVO_PIN);
    elbowServo.attach(ELBOW_SERVO_PIN);
    wristServo.attach(WRIST_SERVO_PIN);
}

void loop() {
    float theta1, theta2, theta3, theta4;

    // 计算逆运动学
    computeIK(target_x, target_y, target_z, theta1, theta2, theta3, theta4);

    // 打印计算的角度
    Serial.printf("Theta1: %.2f, Theta2: %.2f, Theta3: %.2f, Theta4: %.2f\n", theta1, theta2, theta3, theta4);

    // 控制舵机
    baseServo.write(mapServoAngle(theta1));
    shoulderServo.write(mapServoAngle(theta2));
    elbowServo.write(mapServoAngle(theta3));
    wristServo.write(mapServoAngle(theta4));

    delay(2000);  // 等待机械臂移动完成

    // 变换目标点（测试不同位置）
    target_x = (target_x == 10.0) ? 8.0 : 10.0;
    target_y = (target_y == 5.0) ? -5.0 : 5.0;
    target_z = (target_z == 10.0) ? 12.0 : 10.0;
}
